#include <joyos.h>

#include "define.h"
#include "roboturn.h"

/*
 * "turn" turns the robot to the specified angle.
 * Angles are positive when turned counter-clockwise and negative when turned clockwise.
 * The angle is 0 when the direction is right in front of the robot.
 * 
 * @requires none
 * @effects the robot is turned to the specified angle.
 * @modifies none
 */
void turn(uint16_t degree){
	bool turn_left;
	int16_t max_turn_motor_vel = 30;
	int16_t turn_motor_vel;
	
	if (degree >= 0)
		turn_left = true;
	else
		turn_left = false;
		
	calibrateGyro();
	
	if (turn_left){
		int16_t currentAngle = getCurrentAngle();
		while(currentAngle < degree){
			turn_motor_vel = ((degree - currentAngle) / degree) * max_turn_motor_vel;
			motor_set_vel(MOTOR_PORT_RIGHT, turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, -turn_motor_vel);
			currentAngle = getCurrentAngle();
		}
	}
	else{
		int16_t currentAngle = getCurrentAngle();
		while(currentAngle > degree){
			turn_motor_vel = ((degree - currentAngle) / degree) * max_turn_motor_vel;
			motor_set_vel(MOTOR_PORT_RIGHT, turn_motor_vel);
			motor_set_vel(MOTOR_PORT_LEFT, -turn_motor_vel);
			currentAngle = getCurrentAngle();
		}
	}
	
	motor_brake(MOTOR_PORT_LEFT);
	motor_brake(MOTOR_PORT_RIGHT);
}

/*
 * "turnRight" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned clockwise
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */
void turnRight(int16_t degree){
	turn(-degree);
}

/*
 * "turnLeft" is a more user-friendly version of "turn" where the degree put into the function is
 * more positive if turned counter-clockwise
 * 
 * It works exactly the same as "turn"
 *
 * @requires degree >=0
 * @effects the robot is turned to the specified angle to the right
 * @modifies none
 */

void turnLeft(int16_t degree){
	turn(degree);
}

/**
 * turnToOrient turns the robot to the given orientation
 *
 * the currentSide will be updated to the default value, meaning that it
 * is the expected value for normal situation. The currentSide of value 4
 * will be manually adjusted in accident-handler functions.
 * 
 * @requires the robot is not obstructed in turning
 * @effects turns the robot to the given orientation
 * @modifies currentOrient, currentSide
 */
void turnToOrient(uint8_t orientNum){
	if (currentOrient == orientNum)
		{}
	else if ((currentOrient == orientNum - 1) || (currentOrient == orientNum + 3))
		turnRight(90);
	else if ((currentOrient == orientNum - 2) || (currentOrient == orientNum + 2))
		turnRight(180);
	else if ((currentOrient == orientNum -3) || (currentOrient == orientNum + 1))
		turnLeft(90);
	else {
		printf("\nOrient Error");
		pause(5000);
	}
	
	currentSide = currentOrient;
	currentOrient = orientNum;
}

/*
 * "calibrateGyro" lets the robot rest for a brief period of time and reset its angle back to zero
 *
 * @requires none
 * @effects the angle measured by gyroscope is reset to zero
 * @modifies the angle stored in gyroscope
 */
void calibrateGyro(void){
	printf_P (PSTR("\nStabilizing..."));
	pause(2000);
	printf_P (PSTR("\nCalibrating     offset...\n"));
	gyro_init (GYRO_PORT, LSB_MS_PER_DEG, GYRO_CALI_TIME);
}

/**
 * "getCurrentAngle" returns the angle that the robot has turned during the turn
 * 
 * @requires none
 * @effects returns the angle that the robot has turned during the turn
 * @modifies none
 */
int16_t getCurrentAngle(void){
	return gyro_get_degrees();
}
